/**
  ******************************************************************************
  * @file     : Can_Motor_user.c
  * @brief    : can电机的用户层代码源文件
  ******************************************************************************
  * @attention
  * 单纯让电机进行速度环pid(无角度环)时，调用can_motor_set_speed
  * 让电机进行角度环pid时,调用can_motor_set_rel_angle或can_motor_set_abs_angle
  * 停止对指定电机输出电流/电压,调用can_motor_stop
  ******************************************************************************
  * @needToDo
  * 请在main.c初始化代码段调用can_motor_init即可完成所有关于can电机的初始化(函数正确前提下)		
  * 在can_motor_pid_init函数中修改各个电机的pid参数
  **************************************************************************************************
  */
#include "can_motor_user.h"
#include "bsp_can.h"
#include "pid.h"

/*	外部变量&头文件链接区 BEGIN	*/

/*	外部变量&头文件链接区 END	*/


/*	电机参数结构体  BEGIN*/
#if 1
//can1 的电机参数结构体
Motor_measure_t m_0x201_can1 = {0};		//底盘3508
Motor_measure_t m_0x202_can1 = {0};
Motor_measure_t m_0x203_can1 = {0};
Motor_measure_t m_0x204_can1 = {0};
Motor_measure_t m_0x205_can1 = {0};		//防守3508
Motor_measure_t* pmmt_can1_fifo0[can1_fifo0_motor_cnt] = {
	&m_0x201_can1,&m_0x202_can1,
	&m_0x203_can1,&m_0x204_can1,&m_0x205_can1
};

//can2 的电机参数结构体
Motor_measure_t m_0x201_can2 = {0};//运球摩擦轮电机
Motor_measure_t m_0x202_can2 = {0};
Motor_measure_t m_0x203_can2 = {0};//发射拉弹簧电机
Motor_measure_t m_0x204_can2 = {0};
Motor_measure_t m_0x205_can2 = {0};//发射2006
Motor_measure_t* pmmt_can2_fifo0[can2_fifo0_motor_cnt] = {
	&m_0x201_can2,&m_0x202_can2,
	&m_0x203_can2,&m_0x204_can2,&m_0x205_can2
};
#endif
/*	 电机参数结构体   END*/

/*	pid参数结构体  BEGIN*/
#if 1
/*	can1的电机	*/
//速度环pid参数结构体
pid_t spid_0x201_can1 = {0};		
pid_t spid_0x202_can1 = {0};
pid_t spid_0x203_can1 = {0};
pid_t spid_0x204_can1 = {0};
pid_t spid_0x205_can1 = {0};
pid_t* p_spid_can1_fifo0[can1_fifo0_motor_cnt] = {
	&spid_0x201_can1,&spid_0x202_can1,
	&spid_0x203_can1,&spid_0x204_can1,&spid_0x205_can1
};
//角度环pid参数结构体
pid_t apid_0x201_can1 = {0};		
pid_t apid_0x202_can1 = {0};
pid_t apid_0x203_can1 = {0};		
pid_t apid_0x204_can1 = {0};
pid_t apid_0x205_can1 = {0};
pid_t* p_apid_can1_fifo0[can1_fifo0_motor_cnt] = {
	&apid_0x201_can1,&apid_0x202_can1,
	&apid_0x203_can1,&apid_0x204_can1,&apid_0x205_can1
};

/*	can2的电机	*/
//速度环pid参数结构体
pid_t spid_0x201_can2 = {0};		
pid_t spid_0x202_can2 = {0};
pid_t spid_0x203_can2 = {0};
pid_t spid_0x204_can2 = {0};
pid_t spid_0x205_can2 = {0};
pid_t* p_spid_can2_fifo0[can2_fifo0_motor_cnt] = {
	&spid_0x201_can2,&spid_0x202_can2,
	&spid_0x203_can2,&spid_0x204_can2,&spid_0x205_can2
};
//角度环pid参数结构体
pid_t apid_0x201_can2 = {0};		
pid_t apid_0x202_can2 = {0};
pid_t apid_0x203_can2 = {0};
pid_t apid_0x204_can2 = {0};
pid_t apid_0x205_can2 = {0};
pid_t* p_apid_can2_fifo0[can2_fifo0_motor_cnt] = {
	&apid_0x201_can2,&apid_0x202_can2,
	&apid_0x203_can2,&apid_0x204_can2,&apid_0x205_can2
};


motor_angle m2006_angle = {0};	//2006角度累计
#endif
/*	 pid参数结构体   END*/

/********************************************************/
/*		文件内调用函数(不做对外暴露的接口) START		*/
/**
 * @brief 配置好所有电机的参数,默认放在can_motor_init中调用
 */
static void can_motor_measure_init(void)
{
	//底盘
	Can_Motor_measure_t_config(&m_0x201_can1,STOP,ANGLE_NO,0x201);
	Can_Motor_measure_t_config(&m_0x202_can1,STOP,ANGLE_NO,0x202);
	Can_Motor_measure_t_config(&m_0x203_can1,STOP,ANGLE_NO,0x203);
	Can_Motor_measure_t_config(&m_0x204_can1,STOP,ANGLE_NO,0x204);
	//发射
	Can_Motor_measure_t_config(&m_0x205_can1,STOP,ANGLE_NO,0x205);
	Can_Motor_measure_t_config(&m_0x201_can2,STOP,ANGLE_NO,0x201);
	Can_Motor_measure_t_config(&m_0x202_can2,STOP,ANGLE_NO,0x202);
	//运球
	Can_Motor_measure_t_config(&m_0x203_can2,STOP,ANGLE_NO,0x203);
	Can_Motor_measure_t_config(&m_0x204_can2,STOP,ANGLE_NO,0x204);
	//防守
	Can_Motor_measure_t_config(&m_0x205_can2,STOP,ANGLE_NO,0x205);
}

/**
 * @brief 配置好所有电机的pid参数,默认放在can_motor_init中调用
 */
static void can_motor_pid_init()
{
	/*	底盘电机pid	*/
	{
		/*	底盘电机速度环pid	*/
		{
			const uint32_t MAX_CURENT = 15000;		//速度环最大电流输出
			//float p = 5.0f, i = 0.05f, d = 0.002f, f = 0.0f;
			float p = 2.6f, i = 0.01, d = 0.00f, f = 0.1f;
			float intergral_lim = 5000, deadband = 3;
			
			PID_struct_init(&spid_0x201_can1, POSITION_PID, D_FIRST_DISEN,
									MAX_CURENT,	intergral_lim,	deadband,	p ,i,d,f);
			
			PID_struct_init(&spid_0x202_can1, POSITION_PID, D_FIRST_DISEN,
									MAX_CURENT ,	intergral_lim ,	deadband,p ,i ,d,f );
			
			PID_struct_init(&spid_0x203_can1, POSITION_PID, D_FIRST_DISEN,
									MAX_CURENT,	intergral_lim,	deadband,	p ,i, d,f);
			
			PID_struct_init(&spid_0x204_can1, POSITION_PID, D_FIRST_DISEN,
									MAX_CURENT,	intergral_lim,	deadband,	p ,i,d,f);
		}
		/*	底盘电机角度环pid	*/
		{
			const uint32_t MAX_SPEED = 9000;		//角度环最大速度输出
			float p = 10, i = 0, d = 20, f = 0;
			float intergral_lim = 2000, deadband = 0.1;

			PID_struct_init(&apid_0x201_can1, POSITION_PID, D_FIRST_DISEN,
									MAX_SPEED,	intergral_lim,	deadband,	p,i,d,f);
			
			PID_struct_init(&apid_0x202_can1, POSITION_PID, D_FIRST_DISEN,
									MAX_SPEED,	intergral_lim,	deadband,	p,i,d,f);
			
			PID_struct_init(&apid_0x203_can1, POSITION_PID, D_FIRST_DISEN,
									MAX_SPEED,	intergral_lim,	deadband,	p,i,d,f);
			
			PID_struct_init(&apid_0x204_can1, POSITION_PID, D_FIRST_DISEN,
									MAX_SPEED,	intergral_lim,	deadband,	p,i,d,f);
		}
	}
	/*	底盘电机pid	*/
	
	/*	运球电机pid	*/
	{
		/*	运球电机速度环pid	*/
		{
			const uint32_t MAX_CURENT = 10000;		//速度环最大电流输出
			float p = 2, i = 0.1, d = 0.5, f = 0;
			float intergral_lim = 5000, deadband = 5;
			
			PID_struct_init(&spid_0x201_can2, POSITION_PID, D_FIRST_DISEN,
									MAX_CURENT,	intergral_lim,	deadband,	p,i,d,f);
			PID_struct_init(&spid_0x202_can2, POSITION_PID, D_FIRST_DISEN,
									MAX_CURENT,	intergral_lim,	deadband,	p,i,d,f);
		}
	}
	/*	运球电机pid	*/
	
	/*	发射电机pid	*/
	{
		/*	3508速度环pid	*/
		{
			const uint32_t MAX_CURENT = 16000;		//速度环最大电流输出
			float p = 12, i = 0.7, d = 0.5, f = 10;
			float intergral_lim = 20700, deadband = 5;
			
			PID_struct_init(&spid_0x203_can2, POSITION_PID, D_FIRST_EN,	//微分先行
									MAX_CURENT,	intergral_lim,	deadband,	p,i,d,f);
			PID_struct_init(&spid_0x204_can2, POSITION_PID, D_FIRST_EN,	//微分先行
									MAX_CURENT,	intergral_lim,	deadband,	p,i,d,f);
		}
		/*	3508角度环pid	*/
		{
			const uint32_t MAX_SPEED = 5000;		//角度环最大速度输出
			float p = 10, i = 0.5, d = 2, f = 0;
			float intergral_lim = 5000, deadband = 0.1;
			
			PID_struct_init(&apid_0x203_can2, POSITION_PID, D_FIRST_DISEN,	//不微分先行
									MAX_SPEED,	intergral_lim,	deadband,	p,i,d,f);
			PID_struct_init(&apid_0x204_can2, POSITION_PID, D_FIRST_DISEN,	//不微分先行
									MAX_SPEED,	intergral_lim,	deadband,	p,i,d,f);
		}
		/*	2006速度环pid	*/
		{
			const uint32_t MAX_CURENT = 10000;		//速度环最大电流输出,	记住了2006最高10000!!
			float p = 2, i = 0.1, d = 0.5, f = 0;
			float intergral_lim = 5000, deadband = 5;
			
			PID_struct_init(&spid_0x205_can2, POSITION_PID, D_FIRST_EN,	//微分先行
									MAX_CURENT,	intergral_lim,	deadband,	p,i,d,f);
		}
		/*	2006角度环pid	*/
		{
			const uint32_t MAX_SPEED = 8000;		//角度环最大速度输出
			float p = 10, i = 0, d = 20, f = 0;
			float intergral_lim = 2000, deadband = 0.1;
			
			PID_struct_init(&apid_0x205_can2, POSITION_PID, D_FIRST_DISEN,	//不微分先行
									MAX_SPEED,	intergral_lim,	deadband,	p,i,d,f);
		}
	}
	/*	发射电机pid	*/
	
	/*	防守电机pid	*/
	{
		/*	防守电机速度环pid	*/
		{
			const uint32_t MAX_CURENT = 10000;		//速度环最大电流输出
			float p = 2, i = 0.1, d = 0.5, f = 0;
			float intergral_lim = 5000, deadband = 5;
			
			PID_struct_init(&spid_0x205_can1, POSITION_PID, D_FIRST_EN,	//微分先行
									MAX_CURENT,	intergral_lim,	deadband,	p,i,d,f);
		}
		/*	防守电机角度环pid	*/
		{
			const uint32_t MAX_SPEED = 2000;		//角度环最大速度输出
			float p = 10, i = 0, d = 20, f = 0;
			float intergral_lim = 2000, deadband = 0.1;
			
			PID_struct_init(&apid_0x205_can1, POSITION_PID, D_FIRST_DISEN,	//不微分先行
									MAX_SPEED,	intergral_lim,	deadband,	p,i,d,f);
		}
	}
	/*	防守电机pid	*/
}

/*		文件内调用函数(不做对外暴露的接口) END		*/
/********************************************************/

/*	对外暴露接口 START	*/
/**
 * @brief 做好一切与can和电机有关的初始化操作
 */
void can_motor_init()
{
	//配置电机参数
	can_motor_measure_init();
	//配置pid参数
	can_motor_pid_init();
	//开启can1和can2
	can_init(&hcan1);	can_init(&hcan2);
	//配置过滤器
	Can_Motor_filter_config();
}


/**
 * @brief 	(点动式函数)让指定电机以指定速度转(无角度环模式)
 * @param 	pmmt 		电机结构体参数
 * @param 	speed_rpm	转速 单位rpm 
 */
void can_motor_set_speed(Motor_measure_t* pmmt,float speed_rpm)
{
	//设置运转模式
	if(pmmt -> state != RUN) pmmt -> state = RUN;	
	pmmt -> angle_mode = ANGLE_NO;		//无角度环模式
	
	pmmt -> target_speed = speed_rpm;
}

/**
 * @brief 	(点动式函数)让指定电机转过指定角度（速度环内环,角度环外环pid）
 * @param 	pmmt 		电机结构体参数
 * @param 	angle		角度(角度制)	@args 任意实数 
 */
void can_motor_set_rel_angle(Motor_measure_t* pmmt,float angle)
{
	//设置运转模式
	if(pmmt -> state != RUN) pmmt -> state = RUN;
	pmmt -> angle_mode = ANGLE_YES;
	
	pmmt -> target_angle = pmmt -> angle_now + angle;
	pmmt -> angle_total = pmmt -> angle_now;
}

/**
 * @brief 	(点动式函数)让指定电机转到指定角度（速度环内环,角度环外环pid）
 * @param 	pmmt 		电机结构体参数
 * @param 	angle		角度(角度制)	@args 任意实数 
 */
void can_motor_set_abs_angle(Motor_measure_t* pmmt,float angle)
{
	//设置运转模式
	if(pmmt -> state != RUN) pmmt -> state = RUN;
	pmmt -> angle_mode = ANGLE_YES;
	
	pmmt -> target_angle = angle;
	pmmt -> angle_total = pmmt -> angle_now;
}

/**
 * @brief 	(点动式函数)停止对指定电机输出电流/电压
 * @param 	pmmt 		电机参数结构体
 */
void can_motor_stop(Motor_measure_t* pmmt)
{
	//将一切模式清零
	pmmt -> state = STOP;			//电机停止输出
	pmmt -> angle_mode = ANGLE_NO;	//无角度环模式
	
	pmmt -> target_speed = 0;		//清除目标速度
	pmmt -> target_angle = 0;		//清除目标角度
	pmmt -> angle_total = pmmt -> angle_now;	//重置总转过角度
}

//


